#pragma once

#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <math.h>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

__BEGIN_DECLS

// 向量结构体定义
typedef struct {
    double a;       // XY平面角度 (0-360度)
    double b;       // 与XY平面夹角 (度)
    double length;  // 向量长度
    double output;  // 最大绝对值
} VectorAngles;

// 角度转换函数
static inline double degrees(double radians) {
    return radians * 180.0 / M_PI;
}

// 向量计算函数 - 简单优化版本
VectorAngles calculate_vector_angles_simple(double x, double y, double z);

// 打印角度信息
void print_angles_simple(const char* label, double x, double y, double z, VectorAngles angles);

// 控制函数，输入x y z 坐标值，返回计算结果
VectorAngles VectorControlWithResult(int x, int y, int z);

__END_DECLS
